#include 	"Motors.h"
#include 	"STC8H_PWM.h"
#include 	"NVIC.h"
#include 	"Switch.h"

// 初始化
void Motors_init(){
    EAXSFR();

    // 电机 准双向口
	P1_MODE_IO_PU(GPIO_Pin_4 | GPIO_Pin_5 | GPIO_Pin_6 | GPIO_Pin_7);
	P2_MODE_IO_PU(GPIO_Pin_0 | GPIO_Pin_1 | GPIO_Pin_2 | GPIO_Pin_3);
}

static char To_speed(char speed){
    return speed/2.0 + 50;
}

#define PERIOD (MAIN_Fosc / 1000)
static void	PWM_config(MotorSpeed ms) // 0~100 
{
    // 后退                     前进
	// 0     -     50     -     100
	// 最快        停止         最快
    PWMx_InitDefine		PWMx_InitStructure;

    //==========配置PWM1 模式1 左后轮=========
    // PWMx_InitStructure.PWM_Mode    		= Islsft() == 0?CCMRn_PWM_MODE1::CCMRn_PWM_MODE2;
    PWMx_InitStructure.PWM_Mode    		= CCMRn_PWM_MODE1;	//模式,		CCMRn_FREEZE,CCMRn_MATCH_VALID,CCMRn_MATCH_INVALID,CCMRn_ROLLOVER,CCMRn_FORCE_INVALID,CCMRn_FORCE_VALID,CCMRn_PWM_MODE1,CCMRn_PWM_MODE2
	PWMx_InitStructure.PWM_Duty    		= To_speed(ms.LB_Speed) * PERIOD / 100.0;	//PWM占空比时间, 0~Period
	PWMx_InitStructure.PWM_EnoSelect    = ms.LB_Speed != 0 ? (ENO1P | ENO1N) : 0;	//输出通道选择,	ENO1P,ENO1N,ENO2P,ENO2N,ENO3P,ENO3N,ENO4P,ENO4N / ENO5P,ENO6P,ENO7P,ENO8P
	PWM_Configuration(PWM1, &PWMx_InitStructure);
    
    //==========配置PWM2 模式2 右后轮=========
    // PWMx_InitStructure.PWM_Mode    		= Isright() == 0?CCMRn_PWM_MODE2:CCMRn_PWM_MODE1;
    PWMx_InitStructure.PWM_Mode    		= CCMRn_PWM_MODE2;
    PWMx_InitStructure.PWM_Duty    		= To_speed(ms.RB_Speed) * PERIOD / 100.0;
    PWMx_InitStructure.PWM_EnoSelect    = ms.RB_Speed != 0 ? (ENO2P | ENO2N) : 0;
    PWM_Configuration(PWM2, &PWMx_InitStructure);

    //==========配置PWM3 模式1 左前轮=========
    PWMx_InitStructure.PWM_Mode    		= CCMRn_PWM_MODE1;
    PWMx_InitStructure.PWM_Duty    		= To_speed(ms.LF_Speed) * PERIOD / 100.0;
    PWMx_InitStructure.PWM_EnoSelect    = ms.LF_Speed != 0 ? (ENO3P | ENO3N) : 0;
    PWM_Configuration(PWM3, &PWMx_InitStructure);

    //==========配置PWM4 模式2 右前轮=========
    PWMx_InitStructure.PWM_Mode    		= CCMRn_PWM_MODE2;
    PWMx_InitStructure.PWM_Duty    		= To_speed(ms.RF_Speed) * PERIOD / 100.0;
    PWMx_InitStructure.PWM_EnoSelect    = ms.RF_Speed != 0 ? (ENO4P | ENO4N) : 0;
    PWM_Configuration(PWM4, &PWMx_InitStructure);

    //配置PWMA
    PWMx_InitStructure.PWM_Period   = PERIOD - 1;			//周期时间,   0~65535
    PWMx_InitStructure.PWM_DeadTime = 0;					//死区发生器设置, 0~255
    PWMx_InitStructure.PWM_MainOutEnable= ENABLE;			//主输出使能, ENABLE,DISABLE
    PWMx_InitStructure.PWM_CEN_Enable   = ENABLE;			//使能计数器, ENABLE,DISABLE
    PWM_Configuration(PWMA, &PWMx_InitStructure);			//初始化PWM通用寄存器,  PWMA,PWMB

    //切换PWM通道
    PWM1_SW(PWM1_SW_P20_P21);
    PWM2_SW(PWM2_SW_P22_P23);
    PWM3_SW(PWM3_SW_P14_P15);
    PWM4_SW(PWM4_SW_P16_P17);

    //初始化PWMA的中断
    NVIC_PWM_Init(PWMA,DISABLE,Priority_0);
}

// speed：速度 0~100  mode： LEFT_M左前 , MID_M前进 , RIGHT_M右前
void Motors_forward(char speed, MotorsMode mode) {
	MotorSpeed ms = {0};
	
	if (mode == LEFT_M) { // 左
		ms.LF_Speed = 0;
		ms.LB_Speed = speed;
		ms.RF_Speed = speed;
		ms.RB_Speed = 0;
	} else if (mode == RIGHT_M) { // 右
		ms.LF_Speed = speed;
		ms.LB_Speed = 0;
		ms.RF_Speed = 0;
		ms.RB_Speed = speed;
	} else if (mode == MID_M) { // 中
		ms.LF_Speed = speed;
		ms.LB_Speed = speed;
		ms.RF_Speed = speed;
		ms.RB_Speed = speed;
	}
	
	// 调用pwm配置
	PWM_config(ms);
}


// speed：速度 0~100  mode： LEFT_M左后 , MID_M后退 , RIGHT_M右后
void Motors_backward(char speed , MotorsMode mode) {
	MotorSpeed ms = {0}; // 给结构体所有成员赋值为0
	
	if (mode == LEFT_M) { // 左
		ms.LF_Speed = -speed;
		ms.LB_Speed = 0;
		ms.RF_Speed = 0;
		ms.RB_Speed = -speed;
	} else if (mode == RIGHT_M) { // 右
		ms.LF_Speed = 0;
		ms.LB_Speed = -speed;
		ms.RF_Speed = -speed;
		ms.RB_Speed = 0;
	} else if (mode == MID_M) { // 中间
		ms.LF_Speed = -speed;
		ms.LB_Speed = -speed;
		ms.RF_Speed = -speed;
		ms.RB_Speed = -speed;
	}
	
	PWM_config(ms);
}

// speed：速度 0~100  mode： LEFT_M左平移 ，RIGHT_M右平移
void Motors_translate(char speed , MotorsMode mode) {
	MotorSpeed ms = {0}; // 给结构体所有成员赋值为0
	
	if (mode == LEFT_M) { // 左
		ms.LF_Speed = -speed;
		ms.LB_Speed = speed;
		ms.RF_Speed = speed;
		ms.RB_Speed = -speed;
	} else if (mode == RIGHT_M) { // 右
		ms.LF_Speed = speed;
		ms.LB_Speed = -speed;
		ms.RF_Speed = -speed;
		ms.RB_Speed = speed;
	}
	
	PWM_config(ms);
}

// speed：速度 0~100  mode： LEFT_M向左旋转(逆时针) , RIGHT_M向右旋转(顺时针)
void Motors_around(char speed , MotorsMode mode) {
	MotorSpeed ms = {0}; // 给结构体所有成员赋值为0
	
	if (mode == LEFT_M) { // 左(逆时针)
		ms.LF_Speed = -speed;
		ms.LB_Speed = -speed;
		ms.RF_Speed = speed;
		ms.RB_Speed = speed;
	} else if (mode == RIGHT_M) { // 右(顺时针)
		ms.LF_Speed = speed;
		ms.LB_Speed = speed;
		ms.RF_Speed = -speed;
		ms.RB_Speed = -speed;
	}
	
	PWM_config(ms);
}

// speed：速度 0~100  mode： LEFT_M左转 , RIGHT_M右转
void Motors_turn(char speed ,  MotorsMode mode) {
	MotorSpeed ms = {0}; // 给结构体所有成员赋值为0
	
	if (mode == LEFT_M) { // 左
		ms.LF_Speed = speed;
		ms.LB_Speed = -speed*1.25;
		ms.RF_Speed = speed;
		ms.RB_Speed = speed;
	} else if (mode == RIGHT_M) { // 右
		ms.LF_Speed = speed;
		ms.LB_Speed = speed;
		ms.RF_Speed = speed;
		ms.RB_Speed = -speed*1.25;
	}
	
	PWM_config(ms);
}

// 停止
void Motors_stop(){
    MotorSpeed ms = {0};
	// 调用pwm配置
	PWM_config(ms);
}